#include "kinematics.h"

float L1 = 40; //link 1 length mm
float L2 = 55; //link 2 length mm

Matrix matrixMultiply(Matrix A, Matrix B) {
    Matrix result;
    int i, j, k;
    for (i = 0; i < 4; i++) {
        for (j = 0; j < 4; j++) {
            result.m[i][j] = 0;
            for (k = 0; k < 4; k++) {
                result.m[i][j] += A.m[i][k] * B.m[k][j];
            }
        }
    }
    return result;
}

Matrix DH2Matrix(DH params) {
    Matrix matrix;

    float cosTheta = cos(params.theta);
    float sinTheta = sin(params.theta);
    float cosAlpha = cos(params.alpha);
    float sinAlpha = sin(params.alpha);

    matrix.m[0][0] = cosTheta;
    matrix.m[0][1] = -sinTheta * cosAlpha;
    matrix.m[0][2] = sinTheta * sinAlpha;
    matrix.m[0][3] = params.a * cosTheta;
    
    matrix.m[1][0] = sinTheta;
    matrix.m[1][1] = cosTheta * cosAlpha;
    matrix.m[1][2] = -cosTheta * sinAlpha;
    matrix.m[1][3] = params.a * sinTheta;

    matrix.m[2][0] = 0;
    matrix.m[2][1] = sinAlpha;
    matrix.m[2][2] = cosAlpha;
    matrix.m[2][3] = params.b;

    matrix.m[3][0] = 0;
    matrix.m[3][1] = 0;
    matrix.m[3][2] = 0;
    matrix.m[3][3] = 1;

    return matrix;
}

Point getPointFromMatrix(Matrix Mat){
    Point p;
    p.x = Mat.m[0][3];
    p.y = Mat.m[1][3];
    p.z = Mat.m[2][3];
    return p;
}

Point forwardKinematics(JointAngle Angle){
    DH dh1, dh2;
    Matrix m1, m2;
    Point endpoint;
    float joint1_angle = Angle.joint1angle;
    float joint2_angle = Angle.joint2angle;

    dh1.theta = joint1_angle;
    dh1.b = 0;
    dh1.a = L1;
    dh1.alpha = PI/2;

    dh2.theta = -PI/2 + joint2_angle;
    dh2.b = 0;
    dh2.a = L2;
    dh2.alpha = 0;

    m1 = DH2Matrix(dh1);
    m2 = DH2Matrix(dh2);
    endpoint = getPointFromMatrix(matrixMultiply(m1, m2));
    return endpoint;
}

JointAngle inversekinematics(Point endpoint){
    JointAngle Angle;
    float joint2_angle = acos(-endpoint.z/L2);
    float L =  L1 + L2*sin(joint2_angle);
    float joint1_angle = asin(endpoint.y/L);
    Angle.joint1angle = joint1_angle;
    Angle.joint2angle = joint2_angle;
    return Angle;
}